/* 
 * Obstacle avoidance project.
 * 
 * Copyright 2006 POB-Technology. All rights reserved.
 * POB-Technology PROPRIETARY/CONFIDENTIAL.
 * 
 */

#include <pob-eye.h>

#include "bitmap.h"

/** Distance sensor parameter :
 * 
 * Number after the symbol name are the PORTA's pin of your distance sensor.
 *
 * DISTANCE_SENSOR_FRONT : sensor at the front of the pob-bot
 *
 *
 */
#define DISTANCE_SENSOR_FRONT 2 
#define DISTANCE_SENSOR_RIGHT 3 
#define DISTANCE_SENSOR_LEFT  5


/** warning distance value
 *
 */
#define DISTANCE_WARNING 60
//#define DISTANCE_MAX     128


//function for pob-proto and move bot
static void InitPobProto (void);
static void MoveAndStop(UInt8 Way,UInt32 time);
static void MoveBot(UInt8 Way);

//move order for pob-proto board
#define MOVE_RUN		0x05
#define MOVE_BACK		0x0A
#define MOVE_LEFT		0x09
#define MOVE_RIGHT		0x06
#define STOP_BOT		0

//array for display the graphic interface
static GraphicBuffer videoBuffer;
static UInt8         videoBufferArray[LCD_WIDTH*LCD_HEIGHT*BITS];



int main( void )
{
	int sensorFront ;
	int sensorLeft ;
	int sensorRight ;

	int moveOrder ;

	int botBlocked = 0 ;

	float yF, xR,yR,xL,yL;

	InitPOBEYE();
	
	InitLCD();

	InitPobProto();

	//init graphic buffer
	InitGraphicBuffer(&videoBuffer,LCD_WIDTH,LCD_HEIGHT,ONE_BIT,videoBufferArray); 

	while(1)
	{
		//get sensor value
		sensorFront = GetPortAnalog(DISTANCE_SENSOR_FRONT);
		sensorLeft = GetPortAnalog(DISTANCE_SENSOR_LEFT);
		sensorRight = GetPortAnalog(DISTANCE_SENSOR_RIGHT);

		//draw radar and sensor movement on the lcd screen

		ClearGraphicBuffer(&videoBuffer);

		DrawBitmap(0,0,IDB_RADAR,bitmap,&videoBuffer);

		//draw left sensor
		xL = (( 128 - sensorLeft) *80 / 128) * 0.707 ;
		yL = (( 128 - sensorLeft) *80 / 128) * 0.707 ;

		DrawLine( 64,64, 64-xL, 64-yL,  &videoBuffer);

		//draw front sensor
		yF = (( 128 - sensorFront) *64 / 128) ;

		DrawLine( 64,64, 64, 64-yF,  &videoBuffer);

		//draw right sensor
		xR = (( 128 - sensorRight) *80 / 128) * 0.707 ;
		yR = (( 128 - sensorRight) *80 / 128) * 0.707 ;

		DrawLine( 64,64, 64+xR,64-yR,  &videoBuffer);


		DrawLine(64-xL,64-yL, 64, 64-yF, &videoBuffer);
		DrawLine(64,64-yF, 64+xR, 64-yR, &videoBuffer);

		DrawLCD(&videoBuffer);

		//move bot :

		moveOrder = STOP_BOT ;

		if( sensorFront < DISTANCE_WARNING )
		{
			moveOrder = MOVE_RUN ;
		}
		else if( sensorLeft < DISTANCE_WARNING )
		{
			MoveBot(MOVE_LEFT);
			Wait(20000);
		}
		else if( sensorRight < DISTANCE_WARNING )
		{
			MoveBot(MOVE_RIGHT);
			Wait(20000);
		}
		else
		{
			MoveBot(MOVE_BACK);
			Wait(20000);
			botBlocked = 1 ;
		}
		
		if( botBlocked == 0 )
		{
			if( sensorLeft > DISTANCE_WARNING )
			{
				MoveBot(MOVE_RIGHT);
				Wait(20000);
			}

			if( sensorRight > DISTANCE_WARNING )
			{
				MoveBot(MOVE_LEFT);
				Wait(20000);
			}
		}
		
		MoveBot(moveOrder);
	
		botBlocked = 0 ;
	}
	


	return 0;
}


/**
 * Initialize the POB-PROTO board. PORTA set as analogic, PORTC as servomotor.
 */
void InitPobProto (void)
{
	PobProto board;
	

	//to get the position of the analogic joystick, you have to set the PORTA as analogic input
	board.porta=ALL_PORTA_AS_ANA;	

	//all pin of PORTC are configured to manage servomotors
	board.portc=RC7_AS_SERVO |RC6_AS_SERVO |RC3_AS_SERVO |RC2_AS_SERVO |RC1_AS_SERVO |RC0_AS_SERVO;
	

	//RD0 RD1 RD2 RD3 are configured as digitals output to gear DC motor
	//RD4 RD5 RD6 RD7 are configured as digitals input
	board.portd=RD7_AS_DI |RD6_AS_DI |RD5_AS_DI |RD4_AS_DI |RD3_AS_DO |RD2_AS_DO |RD1_AS_DO |RD0_AS_DO;
	
	//set the pob proto
	SetPobProto(&board);

}

/**
 * Move and stop bot.
 *
 * @param Way : move order
 * @param time : time to move
 */
void MoveAndStop( UInt8 Way, UInt32 time )
{
	MoveBot(Way);
	Wait(time);
	MoveBot(STOP_BOT);
}

/**
 * Move the bot
 *
 * @param Way : move bot order
 */
void MoveBot (UInt8 Way)
{	
	SetPortD(Way);
}
